function [Glob,hout,Out ] = formation( t,glob,out,number,ax )
global N
global A1 A2 
global B1 B2 
global C1 C2 
global H1 H2 
global R1 R2 
global Q1 Q2
global X1 X2 
global K11 K12 K21 K22 
global q1 q
global r1
global W1  P alpha gamma  W
global r fi
%disp(t);
global  xydesire
global change_signal L


h1=[0;0];
h2=  0 ;
if ax==1
    if change_signal(number) ==0
        h3=[ r(1,number)*sin(t+fi(1,number));r(1,number)*cos(t+fi(1,number));
                r(2,number)*sin(t+fi(2,number));r(2,number)*cos(t+fi(2,number));
                r(3,number)*sin(t+fi(3,number));r(3,number)*cos(t+fi(3,number));
                r(4,number)*sin(t+fi(4,number));r(4,number)*cos(t+fi(4,number)); ];
    else
        h3( 2*L(1,number)-1 : 2*L(1,number) ) = [-0.85*r(1,number)*xydesire(number, ax)/sqrt(xydesire(number, :)*xydesire(number, :)') ; 0];
        h3( 2*L(2,number)-1 : 2*L(2,number) ) = [-0.35*r(2,number)*xydesire(number, ax)/sqrt(xydesire(number, :)*xydesire(number, :)') ; 0];
        h3( 2*L(3,number)-1 : 2*L(3,number) ) = [  0.35*r(3,number)*xydesire(number, ax)/sqrt(xydesire(number, :)*xydesire(number, :)') ; 0];
        h3( 2*L(4,number)-1 : 2*L(4,number) ) = [  0.85*r(4,number)*xydesire(number, ax)/sqrt(xydesire(number, :)*xydesire(number, :)') ; 0];
        h3=h3';
    end
    
elseif ax==2
    if change_signal(number) ==0
        h3=[r(1,number)*cos(t+fi(1,number));-r(1,number)*sin(t+fi(1,number));
                r(2,number)*cos(t+fi(2,number));-r(2,number)*sin(t+fi(2,number));
                r(3,number)*cos(t+fi(3,number));-r(3,number)*sin(t+fi(3,number));
                r(4,number)*cos(t+fi(4,number));-r(4,number)*sin(t+fi(4,number)); ];
    else
        h3( 2*L(1,number)-1 : 2*L(1,number) ) = [-0.85*r(1,number)*xydesire(number, ax)/sqrt(xydesire(number, :)*xydesire(number, :)') ; 0];
        h3( 2*L(2,number)-1 : 2*L(2,number) ) = [-0.35*r(2,number)*xydesire(number, ax)/sqrt(xydesire(number, :)*xydesire(number, :)') ; 0];
        h3( 2*L(3,number)-1 : 2*L(3,number) ) = [  0.35*r(3,number)*xydesire(number, ax)/sqrt(xydesire(number, :)*xydesire(number, :)') ; 0];
        h3( 2*L(4,number)-1 : 2*L(4,number) ) = [  0.85*r(4,number)*xydesire(number, ax)/sqrt(xydesire(number, :)*xydesire(number, :)') ; 0];
        h3=h3';
    end
    
else    %ax==3
    h3=2*[5;0;
            5;0;
            5;0;
            5;0; ];
end
h4=[h1;h2;h3];

hout=[C1*h2;C2*h3(1:2);C2*h3(3:4);C2*h3(5:6);C2*h3(7:8)];

v1=0; 
v2=0;
if ax==1
    if change_signal(number) ==0
        v3=[-r(1,number)*sin(t+fi(1,number));
            -r(2,number)*sin(t+fi(2,number));
            -r(3,number)*sin(t+fi(3,number));
            -r(4,number)*sin(t+fi(4,number));];
    else
        v3=zeros(4,1);
    end

elseif ax==2
    if change_signal(number) ==0
      v3=[-r(1,number)*cos(t+fi(1,number));
        -r(2,number)*cos(t+fi(2,number));
        -r(3,number)*cos(t+fi(3,number));
        -r(4,number)*cos(t+fi(4,number));];  
    else
        v3=zeros(4,1);
    end

else    %ax==3
    v3=zeros(4,1);
end

v4=[v1;v2;v3;];

beta=zeros(12,1);
error=zeros(11,1);
Out=zeros(6,1);
%u1=1;%leader acc input(m/s^2)
if ax==3
    u1=0;
end
if ax==1
	u1=0;
end
if ax==2
    u1=0;
end    

U=zeros(6,1);

Glob(1:2)=A2*glob(1:2)+B2*u1;
% Glob(1:2)=[0,0;0,0]*glob(1:2)+[1;0]*u1; %匀速直线需要改为一阶系统(仅对虚领导)；
Out(1)=C2*glob(1:2);

for g=2:1+q(number)
  for k=2:1+q(number)
      beta(2*g-1:2*g) = beta(2*g-1:2*g)+W(g,k,number)*(glob(11+(g-1)*2-1:11+(g-1)*2)-glob(11+(k-1)*2-1:11+(k-1)*2));
  end
      beta(2*g-1:2*g) = beta(2*g-1:2*g)+W(g,1,number)*(glob(11+(g-1)*2-1:11+(g-1)*2)-glob(1:2));
end


for g=2:2
 error(3)=glob(22)-h2-X1*glob(12:13);
 Glob(12:13)=A2*glob(12:13)-(alpha+0*beta(2*g-1:2*g)'*P*beta(2*g-1:2*g))*P*beta(2*g-1:2*g)-gamma*B2*sign(B2'*P*beta(2*g-1:2*g)); 
 U(g)=K11*(glob(22)-h2)+K21*glob(12:13)-gamma*R1*sign(R1'*B1'*Q1'*error(3))+v2;
%  U(g)=10*U(g);   %
 Glob(22)=A1*glob(22)+B1*U(g)-H1*(C1*glob(22)-out(g));
 Glob(3)=A1*glob(3)+B1*U(g);
 Out(g)=C1*glob(3);
end 

for g=3:1+q1
 error(2*g-2:2*g-1)=glob(2*g+17:2*g+18)-h3(2*g-5:2*g-4)-X2*glob(2*g+8:2*g+9);
 Glob(2*g+8:2*g+9)=A2*glob(2*g+8:2*g+9)-(alpha+0*beta(2*g-1:2*g)'*P*beta(2*g-1:2*g))*P*beta(2*g-1:2*g)-gamma*B2*sign(B2'*P*beta(2*g-1:2*g));
 U(g)=K12*(glob(2*g+17:2*g+18)-h3(2*g-5:2*g-4))+K22*glob(2*g+8:2*g+9)-gamma*sign((Q2*B2*R2)'*error(2*g-2:2*g-1))+v3(g-2);
%  U(g)=10*U(g);   %
 Glob(2*g+17:2*g+18)=A2*glob(2*g+17:2*g+18)+B2*U(g)-H2*(C2*glob(2*g+17:2*g+18)-out(g));
 Glob(2*g-2:2*g-1)=A2*glob(2*g-2:2*g-1)+B2*U(g);
 Out(g)=C2*glob(2*g-2:2*g-1);
end  




